/* gm_sim_physics.c -- Física do simulador
 * 
 * Copyright (C) 2008, Gustavo Campos Martins <>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <math.h>
#include <semaphore.h>

#include "gm_sim.h"
#include "gm_sim_utils.h"
#include "gm_sim_physics.h"


extern gm_sim_t * self;


/** Inicializa a física do simulador.
 *
 * @param *self Estrutura que contém os dados do simulador.
 */
void
gm_sim_phys_init( )
{
	int i;
	for (i = 0; i < NUM_ROB; i++)
		self->kick[i] = 0;

	/* inicia o espaço */
	self->space = cpSpaceNew();
	self->space->gravity = cpv(0.0f, 0.0f);
	
	/* Otimizações */
	cpSpaceResizeStaticHash(self->space, 20, 2000);
	/* cpSpaceResizeActiveHash(self->space, obj_num, 100); */

	/* inicia os robôs */
	float px[10] = { 120.0, 260.0, 380.0, 380.0, 500.0, 
		     1180.0, 1040.0, 920.0, 920.0, 810.0 };
	float py[10] = { 400.0, 400.0, 575.0, 225.0, 400.0, 
		     400.0, 400.0, 575.0, 225.0, 400.0 };
	for (i = 0; i < NUM_ROB; i++) {
		self->robot[i] = cpBodyNew(INFINITY, INFINITY);
		self->robot[i]->p = cpv(px[i], py[i]);
		self->robot[i]->a = ( i < 5 ) ? 0.0 : PI;
		cpSpaceAddBody(self->space, self->robot[i]);
	}

	cpVect a, b;
	cpShape *seg;
	for (int j = 0; j < NUM_ROB; j++) {
		for (int i=35; i<325; i++) {
			float alfa = i*DEG2RAD;
			a = cpv(cos(alfa)*ROBOT_RADIUS, sin(alfa)*ROBOT_RADIUS);
			b = cpv(cos(alfa + 1*DEG2RAD)*ROBOT_RADIUS, sin(alfa + 1*DEG2RAD)*ROBOT_RADIUS);

			seg = cpSegmentShapeNew(self->robot[j], a, b, 0.0f);
			seg->e = 0.4;
			seg->u = 100.0;
			cpSpaceAddShape(self->space,seg);
		}
		a = cpv(cos(35*DEG2RAD)*ROBOT_RADIUS, sin(35*DEG2RAD)*ROBOT_RADIUS);
		seg = cpSegmentShapeNew(self->robot[j], b, a, 0.0f);
		seg->e = 0.4;
		seg->u = 100.0;
		cpSpaceAddShape(self->space,seg);
	}

	/* inicia a bola */
	self->ball = cpBodyNew(5000.0f, 0.0f);
	self->ball->p = cpv(500.0f, 480.0f);
	self->ball->v = cpv(100.0f, 100.0f);

	cpShape *bShape = cpCircleShapeNew(self->ball, BALL_RADIUS, cpv(0.0f, 0.0f));
	bShape->e = 0.4;
	bShape->u = 100.0;
	cpSpaceAddBody(self->space, self->ball);
	cpSpaceAddShape(self->space, bShape);

	/* inicia as "laterais" do campo */
	/*cpBody *staticBody = cpBodyNew(INFINITY, INFINITY);
	seg = cpSegmentShapeNew(staticBody, FIELD_BL, FIELD_BR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_BR, GOALOPP_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_B, cpv(WORLD_BR.x, GOALOPP_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_B.y), cpv(WORLD_BR.x, GOALOPP_U.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_U.y), GOALOPP_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_U, FIELD_UR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UR, FIELD_UL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UL, GOALOUR_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_U, cpv(WORLD_BL.x, GOALOUR_U.y) , 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_U.y), cpv(WORLD_BL.x, GOALOUR_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_B.y), GOALOUR_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_B, FIELD_BL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);*/
}

/** Atualiza os dados de *self
 *
 * @param *self Dados do simulador
 */
void
gm_sim_phys_update( )
{
	//static int count = 10;
	static float old_ball_posx = 0;

	/* quanto maior for o steps, mais precisos são os cálculos. */
	int steps = 1;
	cpFloat dt = 1.0/60.0/(cpFloat)steps;

	//if (count == 10) {

	//	count = 0;


		/* Se a bola foi chutada */
		for (int i = 0; i < NUM_ROB; i++) {
			if (self->kick[i]) {
				float dist = sqrt( pow(self->ball->p.x - (self->robot[i]->p.x + 
					               cos(self->robot[i]->a)*ROBOT_RADIUS), 2) +
						   pow(self->ball->p.y - (self->robot[i]->p.y +
						       sin(self->robot[i]->a)*ROBOT_RADIUS), 2) );

				if (dist < 15.0)
					cpBodyApplyImpulse(self->ball, 
							cpvmult(cpvsub(self->ball->p, self->robot[i]->p), 15.0), cpvzero);
				self->kick[i] = 0;
			}
		}

		/* atrito da bola com o chão */
		/* ball->v = cpvmult(ball->v, 0.995); */

		//conta os gols
		if (self->ball->p.x < FIELD_BL.x) {
			if (old_ball_posx > FIELD_BL.x)
				self->placar_a++;
		}
		else if (self->ball->p.x > FIELD_BR.x) {
			if (old_ball_posx < FIELD_BR.x)
				self->placar_b++;
		}
		old_ball_posx = self->ball->p.x;

	//}

	//atualiza o Chipmunk
	cpSpaceStep(self->space, dt);
}
